#ifndef MATRIX_H
#define MATRIX_H

#ifndef PI
#define	PI	(3.14159265359f)
#endif

#ifndef EPSILON
#define	EPSILON	(0.0001f)
#endif

#ifndef	SQRT_3
#define SQRT_3	(1.73205080757f)
#endif	SQRT_3

#ifndef	SQRT_2
#define SQRT_2	(1.41421356237f)
#endif	SQRT_2

#ifndef	HALF_SQRT_2
#define HALF_SQRT_2	(0.70710678119f)
#endif	HALF_SQRT_2

class CVector;
class CQuaternion;
class CPlane;
class CMatrix;

//----------------------------------------------------------------------
//	CVector	class
//------------------
//	[]
//	Set
//	Copy			CopyScale
//	Modulus			ModulusSq		
//	GetLength		GetLengthSq		
//	Normalise		SetLength		
//	Scale							
//	GetAz			SetAz			
//	GetDistance						
//	Add				AddFactor		AddScale
//	Sub				GetVector		
//	Blend			Lerp				
//	Dot				Cross			Cross2D
//----------------------------------------------------------------------

class CVector
{
	friend class CMatrix;
	friend class CQuaternion;
public:
	union
	{
		struct
		{
			f32		f[4];
		};
		struct
		{
			f32 x,y,z,w;
		};
	};
public:

	//-----------------------------------
	// access functions
	//-----------------------------------
	inline f32		&operator[]( int idx )
	{
		return f[idx];
	}

	inline const f32	&operator[]( int idx ) const 
	{
		return f[idx];
	}
/*
	CVector()
	{
	}

	~CVector()
	{
	}

	CVector( f32 x, f32 y, f32 z )
	{
		f[0] = x;
		f[1] = y;
		f[2] = z;
	}

	CVector( f32 x, f32 y, f32 z, f32 w )
	{
		f[0] = x;
		f[1] = y;
		f[2] = z;
		f[3] = w;
	}
*/
	inline CVector& operator+=( const CVector& b )
	{
		f[0]+=b.f[0];
		f[1]+=b.f[1];
		f[2]+=b.f[2];
		f[3]+=b.f[3];

		return *this;
	}

	inline CVector& operator-=( const CVector& b )
	{
		f[0]+=b.f[0];
		f[1]+=b.f[1];
		f[2]+=b.f[2];
		f[3]+=b.f[3];

		return *this;
	}

	inline CVector& operator()( f32 X, f32 Y, f32 Z )
	{
		x = X;
		y = Y;
		z = Z;

		return *this;
	}

	inline CVector& operator()( f32 X, f32 Y, f32 Z, f32 W )
	{
		x = X;
		y = Y;
		z = Z;
		w = W;

		return *this;
	}

	//-----------------------------------
	// setup functions
	//-----------------------------------
	inline void			Set( f32 a, f32 b, f32 c )
	{
		f[0] = a;	f[1] = b;	f[2] = c;	f[3] = 0.0f;
	}
	inline void			Set( f32 a, f32 b, f32 c, f32 d )
	{
		f[0] = a;	f[1] = b;	f[2] = c;	f[3] = d;
	}
	inline void			Set( f32 a )
	{
		f[0] = f[1] = f[2] = a;	f[3] = 0.0f;
	}

	//-----------------------------------
	//	rand between
	//-----------------------------------
	inline CVector &CVector::RandBetween( const CVector &a, const CVector &b )
	{
		f[0] = a[0] + (b[0]-a[0])*(rand()/32768.0f);
		f[1] = a[1] + (b[1]-a[1])*(rand()/32768.0f);
		f[2] = a[2] + (b[2]-a[2])*(rand()/32768.0f);
		return *this;
	}

	//-----------------------------------
	//	copying
	//-----------------------------------
	inline void			Copy( const CVector &src )
	{
		f[0] = src.f[0];
		f[1] = src.f[1];
		f[2] = src.f[2];
		f[3] = src.f[3];
	}
	inline void			CopyScale( const CVector &src, f32 fac )
	{
		f[0] = src.f[0]*fac;
		f[1] = src.f[1]*fac;
		f[2] = src.f[2]*fac;
		f[3] = src.f[3];		// do not * by fac
	}
	inline void			CopyScale( const CVector &src, const CVector &fac )
	{
		f[0] = src.f[0]*fac.f[0];
		f[1] = src.f[1]*fac.f[1];
		f[2] = src.f[2]*fac.f[2];
		f[3] = src.f[3]*fac.f[3];
	}

	//-----------------------------------
	//	modulus manipulation
	//-----------------------------------
	inline f32		Modulus() const
	{
		return (f32)sqrt(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
	}
	inline f32		ModulusSq() const
	{
		return (f32)(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
	}
	inline f32		GetLength() const
	{
		return (f32)sqrt(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
	}
	inline f32		GetLengthSq() const
	{
		return (f32)(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
	}
	inline f32		SetLength( f32 l )
	{
		f32	m = Modulus();
		if( m>0 )
		{
			l /= m;
			f[0]*=l;
			f[1]*=l;
			f[2]*=l;
		}
		return m;
	}
	inline f32		Normalise()
	{
		return SetLength(1.0f);
	}
	inline void			Scale( f32 fac )
	{
		f[0] *= fac;
		f[1] *= fac;
		f[2] *= fac;
	}
	inline void			Scale( const CVector &v )
	{
		f[0] *= v.f[0];
		f[1] *= v.f[1];
		f[2] *= v.f[2];
	}

	//-----------------------------------
	//	angles
	//-----------------------------------
	inline f32		GetAz() const
	{
		return (f32)atan2(-f[0],f[2]);
	}
	
	inline void			SetAz( f32 az )
	{
		f32	l = Modulus();
		f[0] = -(f32)(l*sin(az));
		f[1] = 0.0f;
		f[2] = (f32)(l*cos(az));
	}

	inline void		GetAzEl( f32 &az, f32 &el ) const
	{
		f32 d = (f32)sqrt(f[0]*f[0] + f[2]*f[2]);
		if( d==0 )		az = 0;
		else			az = (f32)atan2(-f[0],f[2]);
		if( f[1]==0 )	el = 0;
		else			el = (f32)atan2(f[1],d);
	}
	
	//-----------------------------------
	//	Min and Max
	//-----------------------------------
	inline void		Min( const CVector &a )
	{
		if( a.f[0] < f[0] )	f[0] = a.f[0];
		if( a.f[1] < f[1] )	f[1] = a.f[1];
		if( a.f[2] < f[2] )	f[2] = a.f[2];
		if( a.f[3] < f[3] )	f[3] = a.f[3];
	}
	inline void		Max( const CVector &a )
	{
		if( a.f[0] > f[0] )	f[0] = a.f[0];
		if( a.f[1] > f[1] )	f[1] = a.f[1];
		if( a.f[2] > f[2] )	f[2] = a.f[2];
		if( a.f[3] > f[3] )	f[3] = a.f[3];
	}

	//-----------------------------------
	//	perpendicular and parallel parts
	//-----------------------------------
	inline f32	GetPerpendicularPart( const CVector &v, const CVector &pTo )
	{
		f32	d = pTo.Dot(v);
		f[0] = v.f[0] - d*pTo.f[0];
		f[1] = v.f[1] - d*pTo.f[1];
		f[2] = v.f[2] - d*pTo.f[2];
		f[3] = 0.0f;
		return d;
	}
	inline f32	GetParallelPart( const CVector &v, const CVector &pTo )
	{
		f32	d = pTo.Dot(v);
		f[0] = d*pTo.f[0];
		f[1] = d*pTo.f[1];
		f[2] = d*pTo.f[2];
		f[3] = 0.0f;
		return d;
	}

	//-----------------------------------
	//	get distance to a point
	//-----------------------------------
	inline f32		GetDistance( const CVector &to ) const
	{
		CVector	p;
		p.Sub(this[0],to);
		return p.GetLength();
	}
	inline f32		GetDistanceSq( const CVector &to ) const
	{
		CVector	p;
		p.Sub(this[0],to);
		return p.GetLengthSq();
	}

	//-----------------------------------
	//	add/subtract
	//-----------------------------------
	inline void			Add( const CVector &b )
	{
		f[0]+=b.f[0];
		f[1]+=b.f[1];
		f[2]+=b.f[2];
		f[3]+=b.f[3];
	}

	inline void			AddFactor( const CVector &a, f32 scale )
	{
		f[0] += a.f[0]*scale;
		f[1] += a.f[1]*scale;
		f[2] += a.f[2]*scale;
		f[3] += a.f[3]*scale;
	}
	inline void			AddFactor( const CVector &a, const CVector &b )
	{
		f[0] += a.f[0]*b.f[0];
		f[1] += a.f[1]*b.f[1];
		f[2] += a.f[2]*b.f[2];
		f[3] += a.f[3]*b.f[3];
	}
	inline void			AddScale( const CVector &a, f32 scale )
	{
		f[0] += a.f[0]*scale;
		f[1] += a.f[1]*scale;
		f[2] += a.f[2]*scale;
		f[3] += a.f[3]*scale;
	}
	inline void			AddScale( const CVector &a, const CVector &b )
	{
		f[0] = a.f[0]*b.f[0];
		f[1] = a.f[1]*b.f[1];
		f[2] = a.f[2]*b.f[2];
		f[3] = a.f[3]*b.f[3];
	}

	//-----------------------------------
	//	these functions also work with f32 arrays
	//-----------------------------------
	template <class X>
	inline void			Sub( const X &b )
	{
		f[0]-=b[0];
		f[1]-=b[1];
		f[2]-=b[2];
		f[3]-=b[3];
	}

	template <class X, class Y>
	inline void			Sub( const X &a, const Y &b )
	{
		f[0] = a[0]-b[0];
		f[1] = a[1]-b[1];
		f[2] = a[2]-b[2];
		f[3] = a[3]-b[3];
	}

	template <class X, class Y>
	inline void			GetVector( const X &from, const Y &to )
	{
		f[0] = to[0] - from[0];
		f[1] = to[1] - from[1];
		f[2] = to[2] - from[2];
		f[3] = to[3] - from[3];
	}

	template <class X, class Y>
	inline void			Sum( const X &a, const Y &b )
	{
		f[0] = a[0]+b[0];
		f[1] = a[1]+b[1];
		f[2] = a[2]+b[2];
		f[3] = a[3]+b[3];
	}
	template <class X, class Y>
	inline void			SumFactor( const X &a, const Y &b, f32 bscale )
	{
		f[0] = a[0]+b[0]*bscale;
		f[1] = a[1]+b[1]*bscale;
		f[2] = a[2]+b[2]*bscale;
		f[3] = a[3]+b[3]*bscale;
	}
	//-----------------------------------
	//	blending
	//-----------------------------------
	inline void		Blend( const CVector &a, f32 af, const CVector &b, f32 bf )
	{
		f[0] = a.f[0]*af + b.f[0]*bf;
		f[1] = a.f[1]*af + b.f[1]*bf;
		f[2] = a.f[2]*af + b.f[2]*bf;
		f[3] = a.f[3]*af + b.f[3]*bf;
	}
	
	inline void		Lerp( const CVector &a, const CVector &b, f32 bf )
	{
		f32	af = 1.0f-bf;
		f[0] = a.f[0]*af + b.f[0]*bf;
		f[1] = a.f[1]*af + b.f[1]*bf;
		f[2] = a.f[2]*af + b.f[2]*bf;
		f[3] = a.f[3]*af + b.f[3]*bf;
	}
	
	//-----------------------------------
	//	dot/cross product
	//-----------------------------------
	template <class X>
	inline f32		Dot( const X &b ) const
	{
		return f[0]*b[0] + f[1]*b[1] + f[2]*b[2];
	}
	template <class X>
	inline f32		Dot4( const X &b ) const
	{
		return f[0]*b[0] + f[1]*b[1] + f[2]*b[2] + f[3]*b[3];
	}

	inline void			Cross( const CVector &a, const CVector &b )
	{
		f[0] = a[1]*b[2] - a[2]*b[1];
		f[1] = a[2]*b[0] - a[0]*b[2];
		f[2] = a[0]*b[1] - a[1]*b[0];
		f[3] = 0.0f;
	}
	
	inline bool		Compare4( const CVector &a )
	{
		if(fabs(a[0]-f[0])<EPSILON)
		{
			if(fabs(a[1]-f[1])<EPSILON)
			{
				if(fabs(a[2]-f[2])<EPSILON)
				{
					if(fabs(a[3]-f[3])<EPSILON)
					{
						return true;
					}
				}
			}
		}
		return false;
	}
	
	inline bool		Compare( const CVector &a )
	{
		if(fabs(a[0]-f[0])<EPSILON)
		{
			if(fabs(a[1]-f[1])<EPSILON)
			{
				if(fabs(a[2]-f[2])<EPSILON)
				{
					return true;
				}
			}
		}
		return false;
	}

	inline f32		Cross2D( const CVector &a ) const
	{
		return (a[0]*f[2])-(f[0]*a[2]);
	}

	template <class X, class Y, class Z>
	inline void			MakeCross( const X &a, const Y &b, const Z &c )
	{
		CVector	ab,bc;
		ab.Sub(b,a);
		bc.Sub(c,b);
		Cross(ab,bc);
	}
	template <class X, class Y, class Z>
	inline void			MakeNormal( const X &a, const Y &b, const Z &c )
	{
		MakeCross(a,b,c);
		Normalise();
	}

};

template <class type>
struct TRI 
{

    type v[3]; // Array access useful in for loops
	
	TRI()
	{
		// nothing
	}

	TRI( type v0, type v1, type v2 )
	{
		v[0] = v0;
		v[1] = v1;
		v[2] = v2;
	}
};


template <class type>
struct POLYGON 
{
	int nElem; // number of elements in the polygon
	int maxElem;

	type *pList;

	POLYGON()
	{
		nElem = 0;
		maxElem = 0;
		pList = NULL;
	}

	POLYGON( u32 maxSize )
	{
		maxElem = maxSize;
		pList = new type[maxSize];
	}

	POLYGON( const POLYGON &in )
	{
		CloneData( in );
	}

	~POLYGON()
	{
		DestroyData();
	}

	void CloneData( const POLYGON &in )
	{
		if( !in.pList ) 
			return;

		pList = new type[in.maxElem];
		maxElem = in.maxElem;
		nElem = in.nElem;
		for( int i=0; i<in.nElem; i++ )
		{
			pList[i] = in.pList[i];
		}
	}

	void DestroyData( )
	{
		if( pList )
		{
			delete[] pList;
		}
		pList = NULL;
	}

	POLYGON& operator=( const POLYGON<type> &in )
	{
		if( &in != this )
		{
			DestroyData();

			CloneData( in );
		}
		
		return *this;
	}
};


//----------------------------------------------------------------------
//	CPlane	class derived from CVector
//------------------
//	GetPoint
//	SetPlane
//	PlaneDist
//----------------------------------------------------------------------
class CPlane : public CVector
{
public:

	inline void	GetPoint( CVector &p ) const
	{
		p[0] = f[0]*f[3];
		p[1] = f[1]*f[3];
		p[2] = f[2]*f[3];
		p[3] = 1.0f;
	}

	inline void	GetNormal( CVector &v ) const
	{
		v[0] = f[0];
		v[1] = f[1];
		v[2] = f[2];
		v[3] = 0.0f;
	}

	inline void	SetPlane( f32 a, f32 b, f32 c, f32 d )
	{
		f[0] = a;
		f[1] = b;
		f[2] = c;
		f[3] = d;
	}
	inline void	SetPlane( const CVector &normal, const CVector &point )
	{
		Copy(normal);
		f[3] = -Dot(point);
	}
	inline void	SetPoint( const CVector &point )
	{
		f[3] = -Dot(point);
	}
	inline f32 PlaneDist( const CVector &p ) const
	{
		return Dot(p) + f[3];
	}

	inline bool Clip( const POLYGON<CVector> &in, POLYGON<CVector> *out ) const
	{
		// Make sure our pointer to the out polygon is valid
		assert( out );
		// Make sure we're not passed a degenerate polygon
		assert( in.nElem>2);
		int thisInd=in.nElem-1;
		int nextInd=0;

		int thisRes = GetSide( in.pList[thisInd] );
		int nextRes;

		out->nElem = 0;
		for( nextInd=0; nextInd<in.nElem; nextInd++ )
		{
			nextRes = GetSide( in.pList[nextInd] );
			if( thisRes >=0 )
			{
				// Add the point
				out->pList[out->nElem++] = in.pList[thisInd];
			}
			if( ( thisRes == -1 && nextRes == 1 ) ||
				( thisRes == 1 && nextRes == -1 ) )
			{
				// Add the split point
				GetIntersect(
				in.pList[thisInd],
				in.pList[nextInd], 
				out->pList[out->nElem++] );
			}
			thisInd = nextInd;
			thisRes = nextRes;
		}
		if( out->nElem >= 3 )
		{
			return true;
		}
		return false;
	}

	inline int	GetSide( const CVector &p ) const
	{
		f32	d = PlaneDist(p);
		if( d > EPSILON )		return 1;
		else if( d < -EPSILON )	return -1;
		return 0;
	}

	inline int GetSide( const CVector *p, u16 num ) const
	{
		bool allfront=true, allback=true;
		int res;

		for( int i=0; i<num; i++ )
		{
			res = GetSide( p[i] );

			if( res == -1 )
			{
				allfront = false;
			}
			else if( res == 1 )
			{
				allback = false;
			}
		}
		if( allfront && !allback )
		{
			// All the points were either in front or coplanar
			return 1;
		}
		else if( !allfront && allback )
		{
			// All the points were either in back or coplanar
			return -1;
		}
		else if( !allfront && !allback )
		{
			// Some were in front, some were in back
			return 2;
		}
		// All were coplanar
		return 0;
	}

	inline int	GetSide( const CVector &p, float ep ) const
	{
		f32	d = PlaneDist(p);
		if( d > ep )		return 1;
		else if( d < -ep )	return -1;
		return 0;
	}
	inline int			GetIntersect( const CVector &orig, const CVector &dir, CVector &result, f32 *pt=NULL ) const
	{
		f32	t,t2;
			// t = -PnRo/PnRd
		t2 = Dot(dir);
		if( t2!=0.0 )
		{
			t = -PlaneDist(orig)/t2;
			result.SumFactor(orig,dir,t);
			if( pt )	*pt = t;
			return 1;
		}
		return 0;
	}

	inline int			GetCrossPoint( const CVector &start, const CVector &dest, CVector &result ) const 
	{
		f32	d[2];
		d[0] = PlaneDist(start);
		if( d[0]<0.0f )		return 0;
		d[1] = PlaneDist(dest);
		if( d[1]>=0.0f )	return 0;
		d[0] = d[0]/(d[0]-d[1]);
		result.Lerp(start,dest,d[0]);
		return 1;
	}

	inline void			MakePlane( const CVector &a, const CVector &b, const CVector &c )
	{
		MakeNormal(a,b,c);
		SetPoint(a);
	}
};

//----------------------------------------------------------------------
//	CMatrix	class (array of CVector's)
//------------------
//	[]
//	Identity
//	QuickInvert
//	Copy			CopyNT
//	FromAz			FromEl		FromAzEl
//	FromEuler3		FromLOS		FromAxisAngle
//	Mult			MultNT
//----CVector--------------
//	Mult			MultNT
//	IMult			IMultNT
//----CPlane---------------
//	IMult
//----------------------------------------------------------------------
class CMatrix
{
	friend class CVector;
	friend class CQuaternion;

protected:

	CVector	f[4];

public:

	inline CVector		&operator[]( int idx )			{	return f[idx];	}
	inline const CVector	&operator[]( int idx ) const 	{	return f[idx];	}

	inline void			Identity()
	{
		f[0][0] = f[1][1] = f[2][2] = f[3][3] = 1.0;
		f[0][1] = f[0][2] = f[0][3] = 
		f[1][0] = f[1][2] = f[1][3] = 
		f[2][0] = f[2][1] = f[2][3] = 
		f[3][0] = f[3][1] = f[3][2] = 0.0;
	}
	inline void			FromAzEl( f32 az,f32 el) 
	{
		f32	caz,saz;
		f32	cel,sel;
		cel = (f32)cos(el);
		sel = (f32)sin(el);
		caz = (f32)cos(az);
		saz = (f32)sin(az);
		f[0][0] =  caz;
		f[0][1] =  0;
		f[0][2] =  saz;
		f[0][3] =  0;
		f[1][0] =  saz * sel;
		f[1][1] =  cel;
		f[1][2] = -caz * sel;
		f[1][3] =  0;
		f[2][0] = -saz * cel;
		f[2][1] =  sel;
		f[2][2] =  caz * cel;
		f[2][3] =  0;
		f[3][0] = 
		f[3][1] = 
		f[3][2] = 0.0f;
		f[3][3] = 1.0f;
	}

	inline void			FromEl( f32 el) 
	{
		f32	cel,sel;
		cel = (f32)cos(el);
		sel = (f32)sin(el);
		f[0][0] = 1.0;
		f[0][1] = 0.0;
		f[0][2] = 0.0;
		f[0][3] =  0;
		f[1][0] = 0.0;
		f[1][1] = cel;
		f[1][2] = -sel;
		f[1][3] =  0;
		f[2][0] = 0.0;
		f[2][1] = sel;
		f[2][2] = cel;
		f[2][3] =  0;
		f[3][0] = 
		f[3][1] = 
		f[3][2] = 0.0f;
		f[3][3] = 1.0f;
	}
	inline void			FromAz( f32 az )
	{
		f32 caz, saz;
		caz = (f32)cos(az);
		saz = (f32)sin(az);
		f[0][0] = caz;
		f[0][1] = 0.0f;
		f[0][2] = saz;
		f[0][3] = 0.0f;
		f[1][0] = 0.0f;
		f[1][1] = 1.0f;
		f[1][2] = 0.0f;
		f[1][3] = 0.0f;
		f[2][0] = -saz;
		f[2][1] = 0.0f;
		f[2][2] = caz;
		f[2][3] = 0.0f;
		f[3][0] = 
		f[3][1] = 
		f[3][2] = 0.0f;
		f[3][3] = 1.0f;
	}
	inline void			FromEuler3( const CVector &euler )
	{
		f32	caz,saz;
		f32	cel,sel;
		f32	ctw,stw;
		
		caz = (f32)cos(euler[0]);
		saz = (f32)sin(euler[0]);
		cel = (f32)cos(euler[1]);
		sel = (f32)sin(euler[1]);
		ctw = (f32)cos(euler[2]);
		stw = (f32)sin(euler[2]);
		f[0][0] =  caz * ctw + saz * sel * stw;
		f[0][1] =  cel * stw;
		f[0][2] =  saz * ctw - caz * sel * stw;
		f[0][3] =  0;
		f[1][0] = -caz * stw + saz * sel * ctw;
		f[1][1] =  cel * ctw;
		f[1][2] = -saz * stw - caz * sel * ctw;
		f[1][3] =  0;
		f[2][0] = -saz * cel;
		f[2][1] =  sel;
		f[2][2] =  caz * cel;
		f[2][3] =  0;
		f[3][0] = 
		f[3][1] = 
		f[3][2] = 0.0f;
		f[3][3] = 1.0f;
	}
	inline void			GetEuler( CVector &euler ) const
	{
		f32	saz,caz,sel,cel,stw,ctw,oocel;
		sel = f[2][1];
		cel = (f32)sqrt(1-sel*sel);
	
		if( cel > 0.0f )
		{
			// presume that a twist of over 90' with a facing of over 90' is wrong
			if( f[1][1]<0.0f && f[2][2]<0.0f )
				cel = -cel;
			oocel = 1.0f/cel;
		
			ctw = f[1][1]*oocel;
			stw = f[0][1]*oocel;
			caz = f[2][2]*oocel;
			saz = -f[2][0]*oocel;
		
			euler[0] = (f32)atan2(saz,caz);
			euler[1] = (f32)atan2(sel,cel);
			euler[2] = (f32)atan2(stw,ctw);
		}
		else
		{	// straight up/down - bugger
			euler[0] = euler[2] = 0;
			euler[1] = (f32)asin(Min(Max(sel,-1.0f),1.0f));
		}
	}

	inline void			FromPDU( const CVector &pos, const CVector &dir, const CVector &up )
	{
		f[2].Copy(dir);
		f[2].Normalise();
		f[0].Cross(up,f[2]);
		f[0].Normalise();
		f[1].Cross(f[2],f[0]);
		f[3].Copy(pos);
		f[0][3] = f[1][3] = f[2][3] = 0.0f;
		f[3][3] = 1.0f;
	}
	inline void			FromLOS( const CVector &pos, const CVector &focus )
	{
		CVector	dir,up;
		f32	f,d;
		dir.Sub(focus,pos);
		up.Set(0,1,0);
		d = dir.GetLengthSq();
		f = up.Dot(dir);
		if( f*f/d > 0.95f )
			up.Set(0,0,1);
		FromPDU(pos,dir,up);
	}

	inline void			Copy( const CMatrix &from )
	{
		f[0] = from[0];
		f[1] = from[1];
		f[2] = from[2];
		f[3] = from[3];
	}
	inline void			CopyNT( const CMatrix &from )
	{
		f[0] = from[0];
		f[1] = from[1];
		f[2] = from[2];
	}

	inline void			SimpleInverse( const CMatrix &mat )
	{
		f[0][0] = mat[0][0];
		f[0][1] = mat[1][0];
		f[0][2] = mat[2][0];
		f[0][3] = 0;
		f[1][0] = mat[0][1];
		f[1][1] = mat[1][1];
		f[1][2] = mat[2][1];
		f[1][3] = 0;
		f[2][0] = mat[0][2];
		f[2][1] = mat[1][2];
		f[2][2] = mat[2][2];
		f[2][3] = 0;
		f[3][0] = -(mat[3][0]*f[0][0] + mat[3][1]*f[1][0] + mat[3][2]*f[2][0]);
		f[3][1] = -(mat[3][0]*f[0][1] + mat[3][1]*f[1][1] + mat[3][2]*f[2][1]);
		f[3][2] = -(mat[3][0]*f[0][2] + mat[3][1]*f[1][2] + mat[3][2]*f[2][2]);
		f[3][3] = 1;
	}

	inline void			Mult( const CMatrix &pre, const CMatrix &post )
	{
		f[0][0] = pre[0][0]*post[0][0] + pre[0][1]*post[1][0] + pre[0][2]*post[2][0];
		f[0][1] = pre[0][0]*post[0][1] + pre[0][1]*post[1][1] + pre[0][2]*post[2][1];
		f[0][2] = pre[0][0]*post[0][2] + pre[0][1]*post[1][2] + pre[0][2]*post[2][2];
		f[0][3] = 0;
	
		f[1][0] = pre[1][0]*post[0][0] + pre[1][1]*post[1][0] + pre[1][2]*post[2][0];
		f[1][1] = pre[1][0]*post[0][1] + pre[1][1]*post[1][1] + pre[1][2]*post[2][1];
		f[1][2] = pre[1][0]*post[0][2] + pre[1][1]*post[1][2] + pre[1][2]*post[2][2];
		f[1][3] = 0;
	
		f[2][0] = pre[2][0]*post[0][0] + pre[2][1]*post[1][0] + pre[2][2]*post[2][0];
		f[2][1] = pre[2][0]*post[0][1] + pre[2][1]*post[1][1] + pre[2][2]*post[2][1];
		f[2][2] = pre[2][0]*post[0][2] + pre[2][1]*post[1][2] + pre[2][2]*post[2][2];
		f[2][3] = 0;
	
		f[3][0] = pre[3][0]*post[0][0] + pre[3][1]*post[1][0] + pre[3][2]*post[2][0] + post[3][0];
		f[3][1] = pre[3][0]*post[0][1] + pre[3][1]*post[1][1] + pre[3][2]*post[2][1] + post[3][1];
		f[3][2] = pre[3][0]*post[0][2] + pre[3][1]*post[1][2] + pre[3][2]*post[2][2] + post[3][2];
		f[3][3] = 1;
	}
	inline void			MultNT( const CMatrix &pre, const CMatrix &post )
	{
		f[0][0] = pre[0][0]*post[0][0] + pre[0][1]*post[1][0] + pre[0][2]*post[2][0];
		f[0][1] = pre[0][0]*post[0][1] + pre[0][1]*post[1][1] + pre[0][2]*post[2][1];
		f[0][2] = pre[0][0]*post[0][2] + pre[0][1]*post[1][2] + pre[0][2]*post[2][2];
		f[0][3] = 0;
	
		f[1][0] = pre[1][0]*post[0][0] + pre[1][1]*post[1][0] + pre[1][2]*post[2][0];
		f[1][1] = pre[1][0]*post[0][1] + pre[1][1]*post[1][1] + pre[1][2]*post[2][1];
		f[1][2] = pre[1][0]*post[0][2] + pre[1][1]*post[1][2] + pre[1][2]*post[2][2];
		f[1][3] = 0;
	
		f[2][0] = pre[2][0]*post[0][0] + pre[2][1]*post[1][0] + pre[2][2]*post[2][0];
		f[2][1] = pre[2][0]*post[0][1] + pre[2][1]*post[1][1] + pre[2][2]*post[2][1];
		f[2][2] = pre[2][0]*post[0][2] + pre[2][1]*post[1][2] + pre[2][2]*post[2][2];
		f[2][3] = 0;
	}

	inline CVector&		Mult( const CVector &b, CVector &result ) const 
	{
		result[0] = b[0]*f[0][0] + b[1]*f[1][0] + b[2]*f[2][0] + f[3][0];
		result[1] = b[0]*f[0][1] + b[1]*f[1][1] + b[2]*f[2][1] + f[3][1];
		result[2] = b[0]*f[0][2] + b[1]*f[1][2] + b[2]*f[2][2] + f[3][2];
		return result;
	}
	inline CVector&		IMult( const CVector &b, CVector &result ) const
	{
		CVector	temp;
		temp.Sub(b,f[3]);
		result[0] = temp[0]*f[0][0] + temp[1]*f[0][1] + temp[2]*f[0][2];
		result[1] = temp[0]*f[1][0] + temp[1]*f[1][1] + temp[2]*f[1][2];
		result[2] = temp[0]*f[2][0] + temp[1]*f[2][1] + temp[2]*f[2][2];
		return result;
	}

	inline CVector&		MultNT( const CVector &b, CVector &result ) const 
	{
		result[0] = b[0]*f[0][0] + b[1]*f[1][0] + b[2]*f[2][0];
		result[1] = b[0]*f[0][1] + b[1]*f[1][1] + b[2]*f[2][1];
		result[2] = b[0]*f[0][2] + b[1]*f[1][2] + b[2]*f[2][2];
		return result;
	}
	inline CVector&		IMultNT( const CVector &b, CVector &result ) const
	{
		result[0] = b[0]*f[0][0] + b[1]*f[0][1] + b[2]*f[0][2];
		result[1] = b[0]*f[1][0] + b[1]*f[1][1] + b[2]*f[1][2];
		result[2] = b[0]*f[2][0] + b[1]*f[2][1] + b[2]*f[2][2];
		return result;
	}

	inline CVector&		Mult( CVector &b ) const
	{
		CVector	temp;
		Mult(b,temp);
		b.Copy(temp);
		return b;
	}
	inline CVector&		MultNT( CVector &b ) const
	{
		CVector	temp;
		MultNT(b,temp);
		b.Copy(temp);
		return b;
	}
	inline CVector&		IMult( CVector &b ) const
	{
		CVector	temp;
		IMult(b,temp);
		b.Copy(temp);
		return b;
	}
	inline CVector&		IMultNT( CVector &b ) const
	{
		CVector	temp;
		IMultNT(b,temp);
		b.Copy(temp);
		return b;
	}

	inline CVector&		IMult( const CPlane &p, CPlane &result ) const
	{
		CVector	temp;
		CVector	tp,tn;
		p.GetPoint(temp);
		IMult(temp,tp);
		IMultNT(p,tn);
		result.SetPlane(tn,tp);
		return result;
	}


	inline CVector&		Mult4( const CVector &b, CVector &result ) const 
	{
		result[0] = b[0]*f[0][0] + b[1]*f[1][0] + b[2]*f[2][0] + b[3]*f[3][0];
		result[1] = b[0]*f[0][1] + b[1]*f[1][1] + b[2]*f[2][1] + b[3]*f[3][1];
		result[2] = b[0]*f[0][2] + b[1]*f[1][2] + b[2]*f[2][2] + b[3]*f[3][2];
		result[3] = b[0]*f[0][3] + b[1]*f[1][3] + b[2]*f[2][3] + b[3]*f[3][3];
		return result;
	}
	inline CVector&		Mult4( CVector &b ) const
	{
		CVector	temp;
		Mult4(b,temp);
		b.Copy(temp);
		return b;
	}


};

//-------------------------------------------------------------------------------------------
//	Transposed CMatrix for Direct3D vertex shaders
//-------------------------------------------------------------------------------------------
class TRANSMAT
{
public:
	CVector	f[3];

	inline CVector		&operator[]( int idx )
	{
		return f[idx];
	}
	inline const CVector	&operator[]( int idx ) const 
	{
		return f[idx];
	}

	void	Identity()
	{
		f[0][1] = f[0][2] = f[0][3] = 
		f[1][0] = f[1][2] = f[1][3] = 
		f[2][0] = f[2][1] = f[2][3] = 0.0f;
		f[0][0] = f[1][1] = f[2][2] = 1.0f;
	}

	void	SetPos( const TRANSMAT &pm, const CVector &o )
	{
		f[0][3] = pm.f[0][0]*o[0] + pm.f[0][1]*o[1] + pm.f[0][2]*o[2] + pm.f[0][3];
		f[1][3] = pm.f[1][0]*o[0] + pm.f[1][1]*o[1] + pm.f[1][2]*o[2] + pm.f[1][3];
		f[2][3] = pm.f[2][0]*o[0] + pm.f[2][1]*o[1] + pm.f[2][2]*o[2] + pm.f[2][3];
	}
	void	SetPos( const CVector &p )
	{
		f[0][3] = p[0];
		f[1][3] = p[1];
		f[2][3] = p[2];
	}
	void	SetPivot( const CVector &pos )
	{
		f[0][3] -= pos[0]*f[0][0] + pos[1]*f[0][1] + pos[2]*f[0][2];
		f[1][3] -= pos[0]*f[1][0] + pos[1]*f[1][1] + pos[2]*f[1][2];
		f[2][3] -= pos[0]*f[2][0] + pos[1]*f[2][1] + pos[2]*f[2][2];
	}
};

//-------------------------------------------------------------------------------------------
//	Quaternions
//-------------------------------------------------------------------------------------------
class CQuaternion
{
public:
	f32	f[4];

	//-----------------------------------
	// compress to/from byte representation
	//-----------------------------------
	inline	void		Compress( s8 quat[4] ) const
	{
		quat[0] = (s8)(f[0]*127.0f);
		quat[1] = (s8)(f[1]*127.0f);
		quat[2] = (s8)(f[2]*127.0f);
		quat[3] = (s8)(f[3]*127.0f);
	}

	inline	CQuaternion		&UnCompress( s8 quat[4] )
	{
		f[0] = ((f32)quat[0])/127.0f;
		f[1] = ((f32)quat[1])/127.0f;
		f[2] = ((f32)quat[2])/127.0f;
		f[3] = ((f32)quat[3])/127.0f;
		return *this;
	}

	//-----------------------------------
	// compress to/from word representation
	//-----------------------------------
	inline	void		Compress( s16 quat[4] ) const
	{
		quat[0] = (s16)(f[0]*32767.0f);
		quat[1] = (s16)(f[1]*32767.0f);
		quat[2] = (s16)(f[2]*32767.0f);
		quat[3] = (s16)(f[3]*32767.0f);
	}

	inline	CQuaternion		&UnCompress( s16 quat[4] )
	{
		f[0] = ((f32)quat[0])/32767.0f;
		f[1] = ((f32)quat[1])/32767.0f;
		f[2] = ((f32)quat[2])/32767.0f;
		f[3] = ((f32)quat[3])/32767.0f;
		return *this;
	}

	//-----------------------------------
	// set identity
	//-----------------------------------
	inline	CQuaternion&		Identity( )
	{	f[0] = f[1] = f[2] = 0.0f;	f[3] = 1.0f;	return *this;	}

	//-----------------------------------
	// creation functions, via matrix
	//-----------------------------------
	inline CQuaternion&		FromAz( f32 az )
	{
		CMatrix		m;
		m.FromAz(az);
		fromMatrix(m);
		return *this;
	}
	inline CQuaternion&		FromAzEl( f32 az, f32 el )
	{
		CMatrix		m;
		m.FromAzEl(az,el);
		fromMatrix(m);
		return *this;
	}
	inline CQuaternion&		FromEuler3( const CVector & euler )
	{
		CMatrix		m;
		m.FromEuler3(euler);
		fromMatrix(m);
		return *this;
	}
	//-----------------------------------
	// access functions
	//-----------------------------------
	inline f32		&operator[]( int idx )
	{
		return f[idx];
	}

	inline const f32	&operator[]( int idx ) const 
	{
		return f[idx];
	}

	//-----------------------------------
	// to/from matrix
	//-----------------------------------
	//	adapted from game programming gems 1
	//-----------------------------------
	inline	CMatrix&		toMatrix( CMatrix &m ) const
	{
		f32	norm;
		f32	s;
		f32	xx,yy,zz,xy,xz,yz,wx,wy,wz;

		norm = f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3];
		s =  (norm > 0) ? 2/norm : 0;
		
		xx = f[0] * f[0] * s;
		yy = f[1] * f[1] * s;
		zz = f[2] * f[2] * s;
		xy = f[0] * f[1] * s;
		xz = f[0] * f[2] * s;
		yz = f[1] * f[2] * s;
		wx = f[3] * f[0] * s;
		wy = f[3] * f[1] * s;
		wz = f[3] * f[2] * s;
	
		m[0][0] = 1.0f - (yy + zz);
		m[1][0] = xy + wz;
		m[2][0] = xz - wy;
		
		m[0][1] = xy - wz;
		m[1][1] = 1.0f - (xx + zz);
		m[2][1] = yz + wx;
		
		m[0][2] = xz + wy;
		m[1][2] = yz - wx;
		m[2][2] = 1.0f - (xx + yy);

		m[0][3] = m[1][3] = m[2][3] = 0.0f;
		m[3][3] = 1.0f;
		return m;
	}

	inline	CQuaternion&		fromMatrix( const CMatrix &m )
	{
		f32	Tr;
		f32	fourD;
		int		i,j,k;

		// presume m[3][3] = 1.0
		Tr = m[0][0] + m[1][1] + m[2][2] + 1.0f;

		if( Tr >= 1.0f )
		{
			fourD = 2.0f*(f32)sqrt(Tr);
			f[3] = fourD/4.0f;
			f[0] = (m[2][1] - m[1][2])/fourD;
			f[1] = (m[0][2] - m[2][0])/fourD;
			f[2] = (m[1][0] - m[0][1])/fourD;
		}
		else
		{
			if (m[0][0] > m[1][1])		i = 0;
			else						i = 1;
			if (m[2][2] > m[i][i])		i = 2;

			j = (i+1)%3;
			k = (j+1)%3;
	
			fourD = 2.0f*(f32)sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f );
			f[i] = fourD/4.0f;
			f[j]  = (m[j][i] + m[i][j])/fourD;
			f[k]  = (m[k][i] + m[i][k])/fourD;
			f[3] = (m[k][j] - m[j][k])/fourD;
		}
		return *this;
	}

	//-----------------------------------
	// to transposed matrix
	//-----------------------------------
	inline	TRANSMAT&		toTransMat( TRANSMAT &m ) const
	{
		f32	norm;
		f32	s;
		f32	xx,yy,zz,xy,xz,yz,wx,wy,wz;

		norm = f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3];
		s =  (norm > 0) ? 2/norm : 0;
		
		xx = f[0] * f[0] * s;
		yy = f[1] * f[1] * s;
		zz = f[2] * f[2] * s;
		xy = f[0] * f[1] * s;
		xz = f[0] * f[2] * s;
		yz = f[1] * f[2] * s;
		wx = f[3] * f[0] * s;
		wy = f[3] * f[1] * s;
		wz = f[3] * f[2] * s;
	
		m[0][0] = 1.0f - (yy + zz);
		m[0][1] = xy + wz;
		m[0][2] = xz - wy;
		
		m[1][0] = xy - wz;
		m[1][1] = 1.0f - (xx + zz);
		m[1][2] = yz + wx;
		
		m[2][0] = xz + wy;
		m[2][1] = yz - wx;
		m[2][2] = 1.0f - (xx + yy);
	
		m[0][3] = m[1][3] = m[2][3] = 0.0f;
		return m;
	}

	//-----------------------------------
	// to/from axis/angle
	//-----------------------------------
	inline	CQuaternion		&fromAxisAngle( const CVector &axis, f32 angle )
	{
		f32	l = axis.GetLength();

		if( l==0.0f )
			Identity();
		else
		{
			angle*=0.5f;
			l = ((f32)sin(angle))/l;
			f[0] = axis[0]*l;
			f[1] = axis[1]*l;
			f[2] = axis[2]*l;
			f[3] = (f32)cos(angle);
		}
		return *this;
	}

	//-----------------------------------
	// math operations
	//-----------------------------------
	inline	f32		Normalize()
	{
		f32	l = f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3];
		if( l>0 )
		{
			l = 1.0f/(f32)sqrt(l);
			f[0]*=l;
			f[1]*=l;
			f[2]*=l;
			f[3]*=l;
		}
		else
		{	f[0] = f[1] = f[2] = 0.0f;	f[3] = 1.0f;	}
		return l;
	}

	inline	f32		Dot( const CQuaternion &b ) const
	{
		return f[0]*b[0] + f[1]*b[1] + f[2]*b[2] + f[3]*b[3];
	}

	inline	CQuaternion		&FastSlerp( const CQuaternion &unitFrom, const CQuaternion &unitTo, f32 fraction )
	{
		f32	of = 1.0f-fraction;
		if( unitFrom.Dot(unitTo)<0.0f )
			of *= -1.0f;
		f[0] = unitFrom[0]*of + unitTo[0]*fraction;
		f[1] = unitFrom[1]*of + unitTo[1]*fraction;
		f[2] = unitFrom[2]*of + unitTo[2]*fraction;
		f[3] = unitFrom[3]*of + unitTo[3]*fraction;
		Normalize();
		return *this;
	}

	inline	CQuaternion		&Multiply( const CQuaternion &a, const CQuaternion &b )
	{
		f[0] = a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1];
		f[1] = a[3]*b[1] + a[1]*b[3] + a[2]*b[0] - a[0]*b[2];
		f[2] = a[3]*b[2] + a[2]*b[3] + a[0]*b[1] - a[1]*b[0];
		f[3] = a[3]*b[3] - (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
		return *this;
	}


};

#endif
